#include "PID.h"

PID_T* PID_t;

//************************************
// Method:    	CALCU_SAMA					//< SAMA运算函数
// Programmer:  duan-基于TOP版本
// Time:		2019/04/23
// Returns:   	bool						//< 成功：true；失败：false
// Parameter: 	Model_Base_t * arg			//< 模块参数结构体指针
// Parameter: 	float dt					//< 运算周期
// Parameter: 	bool * DI					//< 数字量输入值数组指针
// Parameter: 	bool * DO					//< 数字量输出值数组指针
// Parameter: 	bool * AI					//< 模拟量输入值数组指针
// Parameter: 	bool * AO					//< 模拟量输出值数组指针
//************************************
//bool CALCU_SAMA(PID)(Model_Base_t* arg, float dt, bool* DI, bool* DO, float* AI, float* AO)
//{
//#define Err_Rate 0.05
//	/****************变量定义区开始**************/
//	float Err = 0;
//	float Up = 0;
//	float Ui = 0;
//	float Ud = 0;
//	float Upid = 0;
//	float Ud_Sum = 0;
//	float PID_Cal = 0;
//
//	/*<<<<<定义输入变量>>>>>*/
//	float PV;			//< 测量值
//	float SP;			//< 设定值
//	float FF;			//< 前馈值
//	float TR;			//< 跟踪值
//	float KP;			//< 比例带
//	float TI;			//< 积分时间
//	float TD;			//< 微分时间
//	float KD;			//< 微分增益
//	float HL;			//< 输出上限
//	float LL;			//< 输出下限
//	float PIDDB;		//< PID死区
//	bool STR;			//< 跟踪方式   0：自动  1：跟踪
//	bool BI;			//< 闭锁增
//	bool BD;			//< 闭锁减
//
//	/*<<<<<定义输出变量>>>>>*/
//	float A_Output_Value = 0.0f;
//	bool D_Output_Value;
//	
//	/*<<<<<定义参数变量>>>>>*/
//	float PVGain;		//< PV增益
//	float PVBias;		//< PV偏置
//	float SPGain;		//< SP增益
//	float SPBias;		//< SP偏置
//	float HighRange;	//< 输出量程上限
//	float LowRange;		//< 输出量程下限
//	float ErrALM;		//< 偏差报警限
//	float A_OutRate;	//< 模拟量输出变化率
//	A_OutMode AO_Mod;	//< 模拟量输出状态  增量式、位置式
//	Direct PID_Dir;		//< 动作方向 正作用、反作用
//	
//	/*<<<<<定义中间变量>>>>>*/
//	float* Last_AO;		//< 上一时刻模拟量输出
//	bool* Last_DO;		//< 上一时刻数字量输出
//	float* Last_Err;	//< 上一时刻偏差值
//	float* Last_Ud;		//< 上一时刻微分作用
//	float* Last_Ui;		//< 上一时刻积分作用
//	float* ErrSum;		//< 偏差累积
//	float* Last_Upid;	//< 上一时刻pid计算值
//	float* Last_KP;		//< 上一时刻比例带					//< MingHua
//
//	/****************变量定义区结束**************/
//
//	NAME(PID)* Obj = (NAME(PID)*)arg;
//
//	/****************变量赋值**************/
//	/*<<<<<输入变量赋值>>>>>*/
//	PV = AI[0];			
//	SP = AI[1];			
//	FF = AI[2];			
//	TR = AI[3];			
//	KP = AI[4];			
//	TI = AI[5];			
//	TD = AI[6];			
//	KD = AI[7];			
//    HL = AI[8];			
//	LL = AI[9];			
//	PIDDB = AI[10];		
//	
//	STR = DI[0];			
//	BI = DI[1];			
//	BD = DI[2];			
//
//	/*<<<<<参数变量赋值>>>>>*/
//	PVGain = Obj->Par.PVGain;		
//	PVBias =Obj->Par.PVBias;		
//	SPGain = Obj->Par.SPGain;		
//	SPBias = Obj->Par.SPBias;		
//	HighRange = Obj->Par.HighRange;	
//	LowRange = Obj->Par.LowRange;		
//	ErrALM = Obj->Par.ErrALM;		
//	A_OutRate = Obj->Par.A_OutRate;	
//	AO_Mod = Obj->Par.AO_Mod;	
//	PID_Dir = Obj->Par.PID_Dir;		
//
//	/*<<<<<中间变量赋值>>>>>*/
//	Last_AO = &(Obj->Sta.Last_AO);			//< 上一时刻模拟量输出
//	Last_DO = &(Obj->Sta.Last_DO);			//< 上一时刻数字量输出
//	Last_Err = &(Obj->Sta.Last_Err);		//< 上一时刻偏差值
//	Last_Ud = &(Obj->Sta.Last_Ud);			//< 上一时刻微分作用
//	Last_Ui = &(Obj->Sta.Last_Ui);			//< 上一时刻积分作用
//	ErrSum = &(Obj->Sta.ErrSum);			//< 偏差累积
//	Last_Upid = &(Obj->Sta.Last_Upid);		//< 上一时刻模拟量
//	Last_KP = &(Obj->Sta.Last_KP);			//< 上一时刻比例带				
//	/**************变量赋值结束************/
//
//	/********************************************/
//	/**************算法功能实现开始**************/
//	/********************************************/
//	if (HL > 1000)							//< 模拟量输出上下限限值			
//		HL = 1000;
//	if (LL < -1000)
//		LL = -1000;
//
//	PV = PV * PVGain + PVBias;				//< 测量值标度变换
//	SP = SP * SPGain + SPBias;				//< 设定值标度变换
//
//	if (PID_Dir == Positive)				//<  正作用
//		Err = PV - SP;
//	else
//		Err = SP - PV; 
//
//	if (Err > ErrALM || Err < -ErrALM)		//< 偏差报警							
//		D_Output_Value = true;
//	else if ((Last_Err[0] >= ErrALM) && ((ErrALM - Err) / ErrALM <= Err_Rate))
//		D_Output_Value = true;
//	else if ((Last_Err[0] <= -ErrALM) && ((Err - (-ErrALM)) / ErrALM <= Err_Rate))
//		D_Output_Value = true;
//	else 
//		D_Output_Value = false;
//
//	if (STR == true)						//< 跟踪
//	{
//		A_Output_Value = TR;				//< AO输出跟踪值
//
//		Last_Ui[0] = 0;
//		Last_Err[0] = Err;
//		Last_Ud[0] = 0;
//		Last_AO[0] = TR - FF;					
//		Last_KP[0] = KP;
//	}	//< "跟踪"end
//	else									//< 自动
//	{
//		if (KP != 0 && Last_KP[0] != 0)								//< 比例作用	
//			Up = Err / KP - Last_Err[0] / Last_KP[0];					
//		else
//			Up = 0;
//	
//		if (TI != 0)												//< 积分作用
//			Ui = (dt / TI) * Err;				
//		else
//			Ui = 0;
//		
//		if (TD != 0)												//< 微分作用
//		{
//			Ud_Sum = (TD * Last_Ud[0] + KD * TD * (Err - Last_Err[0])) / (dt + TD);
//			Ud = Ud_Sum - Last_Ud[0];			
//		}	
//		else
//			Ud = 0;
//		if (KD == 0)
//		{
//			Ud_Sum = 0;
//			Ud = 0;
//			Last_Ud[0] = 0;
//		}
//
//		PID_Cal = Up + Ui + Ud;						//< PID作用计算值					
//
//		if (fabs(Err) <= PIDDB)						//< |偏差|小于死区				
//			PID_Cal = 0;
//
//		if (AO_Mod == Incremental_PID)				//< 增量式
//			Upid = PID_Cal;
//		if (AO_Mod == Positional_PID)				//< 位置式					
//			Upid = Last_Upid[0] + PID_Cal;
//
//		A_Output_Value = Upid + FF;					//< 计算实际输出值
//		A_OutRate = (float)fabs(A_OutRate);			//< 模块实际输出值输出限速						
//		if (fabs(A_Output_Value - Last_AO[0]) / dt > A_OutRate)
//		{
//			if ((A_Output_Value - Last_AO[0]) > 0)
//				A_Output_Value = Last_AO[0] + A_OutRate * dt;
//			if ((A_Output_Value - Last_AO[0]) < 0)
//				A_Output_Value = Last_AO[0] - A_OutRate * dt;
//		}
//
//		if (A_Output_Value > HL)					//< 模块实际输出值限值				
//			A_Output_Value = HL;
//		else if (A_Output_Value < LL)
//			A_Output_Value = LL;
//
//		
//		if (Upid > HL)								//< Upid超限										
//			Upid = HL;
//		else if (Upid < LL)
//			Upid = LL;
//
//		if (BI == true)								//< 闭锁增										
//		{
//			if (A_Output_Value > Last_AO[0])
//				A_Output_Value = Last_AO[0];
//		}
//		if (BD == true)								//< 闭锁减
//		{
//			if (A_Output_Value < Last_AO[0])
//				A_Output_Value = Last_AO[0];
//		}
//
//		Last_AO[0] = A_Output_Value;												
//		Last_Upid[0] = Upid;
//		Last_KP[0] = KP;
//		Last_Err[0] = Err;
//		Last_Ui[0] = Ui;						//< 存积分差值
//		Last_Ud[0] = Ud_Sum;					//< 存微分总值											
//	}	//< "自动“end
//
//	/********************************************/
//	/**************算法功能实现结束**************/
//	/********************************************/
//
//	/****************运算结果输出**************/
//	AO[0] = A_Output_Value;
//	DO[0] = D_Output_Value;
//	/**************运算结果输出完毕************/
//
//#undef Err_Rate
//	return true;
//}

//************************************
// Method:    	CALCU_SAMA					//< SAMA运算函数
// Programmer:  duan-基于明华版本APID
// Time:		2019/04/23
// Returns:   	bool						//< 成功：true；失败：false
// Parameter: 	Model_Base_t * arg			//< 模块参数结构体指针
// Parameter: 	float dt					//< 运算周期
// Parameter: 	bool * DI					//< 数字量输入值数组指针
// Parameter: 	bool * DO					//< 数字量输出值数组指针
// Parameter: 	bool * AI					//< 模拟量输入值数组指针
// Parameter: 	bool * AO					//< 模拟量输出值数组指针
//************************************
bool CALCU_SAMA(PID)(Model_Base_t* arg, float dt, bool* DI, bool* DO, float* AI, float* AO)
{
#define Err_Rate 0.05
	/****************变量定义区开始**************/
	float Err = 0;
	float Up = 0;
	float Ui = 0;
	float Ud = 0;
	float Upid = 0;
	float Ud_Sum = 0;
	float PID_Cal = 0;

	/*<<<<<定义输入变量>>>>>*/
	float PV;			//< 测量值
	float SP;			//< 设定值
	float FF;			//< 前馈值
	float TR;			//< 跟踪值
	float KP;			//< 比例带
	float TI;			//< 积分时间
	float TD;			//< 微分时间
	float KD;			//< 微分增益
	float HL;			//< 输出上限
	float LL;			//< 输出下限
	float PIDDB;		//< PID死区
	bool STR;			//< 跟踪方式   0：自动  1：跟踪
	bool BI;			//< 闭锁增
	bool BD;			//< 闭锁减

	/*<<<<<定义输出变量>>>>>*/
	float A_Output_Value = 0.0f;
	bool D_Output_Value;

	/*<<<<<定义参数变量>>>>>*/
	float PVGain;		//< PV增益
	float PVBias;		//< PV偏置
	float SPGain;		//< SP增益
	float SPBias;		//< SP偏置
	float HighRange;	//< 输出量程上限
	float LowRange;		//< 输出量程下限
	float ErrALM;		//< 偏差报警限
	float A_OutRate;	//< 模拟量输出变化率
	A_OutMode AO_Mod;	//< 模拟量输出状态  增量式、位置式
	Direct PID_Dir;		//< 动作方向 正作用、反作用

	/*<<<<<定义中间变量>>>>>*/
	float* Last_AO;		//< 上一时刻模拟量输出
	bool* Last_DO;		//< 上一时刻数字量输出
	float* Last_Err;	//< 上一时刻偏差值
	float* Last_Ud;		//< 上一时刻微分作用
	float* Last_Ui;		//< 上一时刻积分作用
	float* ErrSum;		//< 偏差累积
	float* Last_Upid;	//< 上一时刻pid计算值
	float* Last_KP;		//< 上一时刻比例带					

	/****************变量定义区结束**************/

	NAME(PID)* Obj = (NAME(PID)*)arg;

	/****************变量赋值**************/
	/*<<<<<输入变量赋值>>>>>*/
	PV = AI[0];
	SP = AI[1];
	FF = AI[2];
	TR = AI[3];
	KP = AI[4];
	TI = AI[5];
	TD = AI[6];
	KD = AI[7];
	HL = AI[8];
	LL = AI[9];
	PIDDB = AI[10];

	STR = DI[0];
	BI = DI[1];
	BD = DI[2];

	/*<<<<<参数变量赋值>>>>>*/
	PVGain = Obj->Par.PVGain;
	PVBias = Obj->Par.PVBias;
	SPGain = Obj->Par.SPGain;
	SPBias = Obj->Par.SPBias;
	HighRange = Obj->Par.HighRange;
	LowRange = Obj->Par.LowRange;
	ErrALM = Obj->Par.ErrALM;
	A_OutRate = Obj->Par.A_OutRate;
	AO_Mod = Obj->Par.AO_Mod;
	PID_Dir = Obj->Par.PID_Dir;

	/*<<<<<中间变量赋值>>>>>*/
	Last_AO = &(Obj->Sta.Last_AO);			//< 上一时刻模拟量输出
	Last_DO = &(Obj->Sta.Last_DO);			//< 上一时刻数字量输出
	Last_Err = &(Obj->Sta.Last_Err);		//< 上一时刻偏差值
	Last_Ud = &(Obj->Sta.Last_Ud);			//< 上一时刻微分作用
	Last_Ui = &(Obj->Sta.Last_Ui);			//< 上一时刻积分作用
	ErrSum = &(Obj->Sta.ErrSum);			//< 偏差累积
	Last_Upid = &(Obj->Sta.Last_Upid);		//< 上一时刻模拟量
	Last_KP = &(Obj->Sta.Last_KP);			//< 上一时刻比例带				
	/**************变量赋值结束************/

	/********************************************/
	/**************算法功能实现开始**************/
	/********************************************/
	float pid_cal_low_limit;
	if (FF > 0)
		pid_cal_low_limit = LL - fabs(FF);
	else
		pid_cal_low_limit = LL;

	if (HL > 1000)							//< 模拟量输出上下限限值			
		HL = 1000;
	if (LL < -1000)
		LL = -1000;

	PV = PV * PVGain + PVBias;				//< 测量值标度变换
	SP = SP * SPGain + SPBias;				//< 设定值标度变换

	if (PID_Dir == Positive)				//<  正作用
		Err = PV - SP;
	else
		Err = SP - PV;

	if (Err > ErrALM || Err < -ErrALM)		//< 偏差报警							
		D_Output_Value = true;
	else if ((Last_Err[0] >= ErrALM) && ((ErrALM - Err) / ErrALM <= Err_Rate))
		D_Output_Value = true;
	else if ((Last_Err[0] <= -ErrALM) && ((Err - (-ErrALM)) / ErrALM <= Err_Rate))
		D_Output_Value = true;
	else
		D_Output_Value = false;

	if (STR == true)						//< 跟踪
	{
		A_Output_Value = TR;				//< AO输出跟踪值

		Last_Ui[0] = 0;
		Last_Err[0] = Err;
		Last_Ud[0] = 0;
		Last_AO[0] = TR - FF;
		Last_KP[0] = KP;
		Last_Upid[0] = Last_AO[0];			//< duan20200210新增
	}	//< "跟踪"end
	else									//< 自动
	{
		if (KP != 0 && Last_KP[0] != 0)								//< 比例作用	
			Up = Err / KP - Last_Err[0] / Last_KP[0];
		else
			Up = 0;

		if (TI != 0 )												//< 积分作用
			Ui = (dt / TI) * Err;
		else
			Ui = 0;

		if (TD != 0)												//< 微分作用
		{
			Ud_Sum = (TD * Last_Ud[0] + KD * TD * (Err - Last_Err[0])) / (dt + TD);
			Ud = Ud_Sum - Last_Ud[0];
		}
		else
			Ud = 0;
		if (KD == 0)
		{
			Ud_Sum = 0;
			Ud = 0;
			Last_Ud[0] = 0;
		}

		////< ShiKe20200827
		//if (AO_Mod == Positional_PID)				//< 位置式	
		//{
		//	if (Last_Upid[0] + PID_Cal > HL)
		//		Ui = Last_Upid[0] + PID_Cal - Up - Ud;
		//	else if (Last_Upid[0] + PID_Cal < LL)
		//		Ui = Last_Upid[0] + PID_Cal - Up - Ud;
		//}
		////< ShiKe20200827

		PID_Cal = Up + Ui + Ud;						//< PID作用计算值					

		if (fabs(Err) <= PIDDB)						//< |偏差|小于死区				
			PID_Cal = 0;

		if (BI == true)								//< 闭锁增
			PID_Cal = PID_Cal > 0 ? 0 : PID_Cal;
		if (BD == true)								//< 闭锁减
			PID_Cal = PID_Cal < 0 ? 0 : PID_Cal;

		if (AO_Mod == Incremental_PID)				//< 增量式
			Upid = PID_Cal;
		if (AO_Mod == Positional_PID)				//< 位置式					
			Upid = Last_Upid[0] + PID_Cal;

		A_Output_Value = Upid + FF;					//< 计算实际输出值

		A_OutRate = (float)fabs(A_OutRate);			//< 模块实际输出值输出限速						
		if (fabs(A_Output_Value - Last_AO[0]) / dt > A_OutRate)
		{
			if ((A_Output_Value - Last_AO[0]) > 0)
				A_Output_Value = Last_AO[0] + A_OutRate * dt;
			if ((A_Output_Value - Last_AO[0]) < 0)
				A_Output_Value = Last_AO[0] - A_OutRate * dt;
		}

		if (A_Output_Value > HL)					//< 模块实际输出值限值				
			A_Output_Value = HL;
		else if (A_Output_Value < LL)
			A_Output_Value = LL;


		if (Upid > HL)		//< Upid超限	//< ShiKe20200716注释：虽然这个超限判断是在AO计算完后限制的，但它起作用了，作用到了Last_Upid[0]															
			Upid = HL;							//< ShiKe20200716注释：这就导致下一次计算PID时  Last_Upid[0]+PID_Cal 会受到限制
		else if (Upid < pid_cal_low_limit)					//< ShiKe20200716注释：复现这个问题，可以取消注释这段代码，然后PID下限设置为10，加入5的前馈，然后控一个1/(10s+1)对象，就会发现AO输出最终没法降到10
			Upid = pid_cal_low_limit;							//< ShiKe20200716注释：所以注释了这段代码


		Last_AO[0] = A_Output_Value;
		Last_Upid[0] = Upid;
		Last_KP[0] = KP;
		Last_Err[0] = Err;
		Last_Ui[0] = Ui;						//< 存积分差值
		Last_Ud[0] = Ud_Sum;					//< 存微分总值											
	}	//< "自动“end

	/********************************************/
	/**************算法功能实现结束**************/
	/********************************************/

	/****************运算结果输出**************/
	AO[0] = A_Output_Value;
	DO[0] = D_Output_Value;
	/**************运算结果输出完毕************/

#undef Err_Rate
	return true;
}

//************************************
// Method:    	BCPARA						//< 广播SAMA参数变量函数（无参数需要广播的模块直接将BCPara函数设置为NULL即可）
// Programmer:  lww
// Time:		2018/11/16
// Returns:   	int							//< 成功：返回打包字节个数；失败：返回0
// Parameter: 	Model_Base_t * arg			//< 模块参数结构体指针
// Parameter: 	void ** Data				//< 广播SAMA参数的数据包的指针
//************************************
int BC_PARA(PID)(Model_Base_t* arg, void** Data)
{
	NAME(PID)* Obj = (NAME(PID)*)arg;
	BC_PARA_PUBLIC(Obj);
	return 0;
}



//************************************
// Method:    	BAK_STA						//< 备份SAMA运算中间参数
// Programmer:  lww
// Time:		2018/11/16
// Returns:   	int							//< 成功：返回打包字节个数；失败：返回0
// Parameter: 	Model_Base_t * arg			//< 指定模块类型链表的指针
// Parameter: 	void ** Data				//< 存放数据包当前指针位置的地址
//************************************
int BAK_STA(PID)(Model_Base_t* arg, void** Data)
{
	NAME(PID)* Obj = (NAME(PID)*)arg;
	BAK_STA_PRIVATE(Obj);
	return 0;
}



//************************************
// Method:    	ANA_STA						//< 解析备份的SAMA运算中间参数
// Programmer:  lww
// Time:		2018/11/16
// Returns:   	int							//< 成功：返回解析字节个数；失败：返回0
// Parameter: 	Model_Base_t * arg			//< 模块参数结构体指针
// Parameter: 	void ** Data				//< 存放数据包当前指针位置的地址
//************************************
int ANA_STA(PID)(Model_Base_t* arg, void** Data)
{
	NAME(PID)* Obj = (NAME(PID)*)arg;
	ANA_STA_PRIVATE(Obj);
	return 0;
}



//************************************
// Method:    	CHANGE_PAR				//< 在线修改模块参数函数
// Programmer:  lww
// Time:		2018/11/16
// Returns:   	int						//< 读取字节长度计数值
// Parameter: 	Model_Base_t* arg		//< 指定模块类型链表的指针
// Parameter: 	void** Data				//< 存放数据包当前指针位置的地址
//************************************
int CHANGE_PAR(PID)(Model_Base_t* arg, void** Data)
{
	int _iLen = 0;
	MEMREAD(((NAME(PID)*)arg)->Par, Data[0]);
	return _iLen;
}



//************************************
// Method:    	INIT_STAPAR					//< 初始化模块运算中间参数
// Programmer:  lww
// Time:		2018/11/16
// Parameter: 	Model_Base_t* arg			//< 模块参数结构体指针
//************************************
void INIT_STAPAR(PID)(Model_Base_t* arg)
{
	NAME(PID)* name = (NAME(PID)*)arg;
	/****************初始化中间变量结构体**************/
	memset(&(name->Sta), 0, sizeof(name->Sta));
	/**************初始化中间变量结构体结束************/

	/****************对中间变量结构体的相关参数进行赋值**************/

	/**************对中间变量结构体的相关参数进行赋值结束************/
}
